#include "TSDF.cuh"
// #include <memory>
#include <opencv2/hdf.hpp>

#define CVUI_IMPLEMENTATION
#include "cvui.h"
#define WINDOW_NAME "CVUI Hello World!"
class ViewCtl
{
public:
    bool interactive_mode_ = true;
    bool exit_ = false;
    bool pause_ = false;
    static void KeyboardCallback(const cv::viz::KeyboardEvent &event, void *pthis)
    {
        ViewCtl &kinfu = *static_cast<ViewCtl *>(pthis);

        if (event.action != cv::viz::KeyboardEvent::KEY_DOWN)
            return;
        if (event.code == 'i' || event.code == 'I')
            kinfu.interactive_mode_ = !kinfu.interactive_mode_;

        switch (event.action)
        {
        case 'q':
            kinfu.exit_ = true;
            break;
        case 'p':
            kinfu.pause_ = !kinfu.pause_;
            break;
        case 't':
            assert(0);
            break;
        default:
            break;
        }
        // std::cout << event.code << std::endl;
    }
};
void SaveVoxelGrid2SurfacePointCloud(cv::Mat &p, cv::Mat &col, const std::string &file_name)
{

    FILE *fp = fopen(cv::format("%s.ply", file_name.c_str()).c_str(), "w");
    fprintf(fp, "ply\n");
    fprintf(fp, "format binary_little_endian 1.0\n");
    fprintf(fp, "element vertex %ld\n", p.rows);
    fprintf(fp, "property float x\n");
    fprintf(fp, "property float y\n");
    fprintf(fp, "property float z\n");
    fprintf(fp, "property uchar red\n");
    fprintf(fp, "property uchar green\n");
    fprintf(fp, "property uchar blue\n");
    fprintf(fp, "end_header\n");
    cout << file_name << ":" << p.rows << endl;
    for (int i = 0; i < p.rows; i++)
    {
        fwrite(p.ptr<cv::Vec3f>(i, 0), sizeof(float), 3, fp);
        fwrite(col.ptr<cv::Vec3b>(i, 0), sizeof(uchar), 3, fp);
    }
    fclose(fp);
}
int main(int c, char *avg[])
{
    // loguru::g_stderr_verbosity = 9; // print everything
    ck(cudaGetLastError());

    cv::viz::Viz3d viz1("viz1");
    viz1.showWidget("Coordinate", cv::viz::WCoordinateSystem());
    ViewCtl vctl;
    viz1.registerKeyboardCallback(ViewCtl::KeyboardCallback, &vctl);

    cv::FileStorage fs("/home/u20/桌面/rec/ui/MarkerSLAM/camera.yml", cv::FileStorage::READ);

    std::fstream fss("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02/file.txt", std::ios::in);
    std::vector<string> as;
    for (int i = 0; i < 2000; ++i)
    {
        string s1;
        fss >> s1;
        as.push_back("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02" + s1);
    }
    fss.close();

    if (!fs.isOpened())
    {
        // cout << " 没有" << datapath + "/info.yaml" << endl;
        assert(0);
    }
    int2 imgsz;
    Mat cam_K;

    fs["camera_matrix"] >> cam_K;
    fs["image_height"] >> imgsz.x;
    fs["image_width"] >> imgsz.y;
    float4 cam = make_float4(cam_K.at<float>(0, 0), cam_K.at<float>(1, 1), cam_K.at<float>(0, 2), cam_K.at<float>(1, 2));

    TSDF tsdf(make_uint3(512, 512, 512), imgsz);
    tsdf.pintr = new Camera(cam);
    tsdf.pintr->sca = 1.0f / 1000.0f;
    cv::FileStorage fs3("/home/u20/ros/src/MarkerSLAM/build/mpose4.yml", cv::FileStorage::READ);
    Mat poses, idx, ALLpaths_rgbs;
    vector<string> paths_rgbs;
    vector<string> paths_deps;

    fs3["paths_rgbs"] >> paths_rgbs;
    fs3["paths_deps"] >> paths_deps;
    fs3["ALLpaths_rgbs"] >> ALLpaths_rgbs;

    fs3["pose"] >> poses;
    fs3["idx"] >> idx;
    fs3.release();

    int *pindx = idx.ptr<int>();
    float *pfsss = ALLpaths_rgbs.ptr<float>();

    cv::Mat frame = cv::Mat(1000, 1600, CV_8UC3);
    // Init cvui and tell it to create a OpenCV window, i.e. cv::namedWindow(WINDOW_NAME).
    cvui::init(WINDOW_NAME);

    for (int i = 0; i < paths_rgbs.size(); i++)
    {
        // int j = pindx[i * 3];

        cv::Mat rgb = cv::imread(paths_rgbs[i]);
        cv::Mat depth = cv::imread(paths_deps[i], 2);
        cv::Affine3f pose(pfsss + i * 16);
        cout << pose.matrix << endl;
        // 读取图片
        // cv::Mat rgb = cv::imread(cv::format("%s/rgb/%d.png", path.c_str(), i));
        // cv::Mat depth = cv::imread(cv::format("%s/depth/%dd.png", path.c_str(), i), cv::IMREAD_ANYDEPTH);
        // cv::Affine3f pose = cv::Affine3f::Identity(); // dt.pose.getvectorPose(i);
        tsdf.addScan(depth, rgb, pose.inv());
        // // //显示当前TSDF
        Mat cpu_cloud2;
        Mat cpu_color;
        tsdf.exportCloud(cpu_cloud2, cpu_color);
        viz1.showWidget("depthmode222", cv::viz::WCloud(cpu_cloud2, cpu_color));
        if (vctl.interactive_mode_)
            viz1.setViewerPose(pose.inv());

        // // 显示当前帧深度图对应的点云
        // Mat depth_color, cpu_cloud;
        // tsdf.depth2cam(depth, rgb, depth_color, cpu_color, cv::Affine3f::Identity());
        // cv::Affine3f viewpose = cv::Affine3f::Identity();
        // viz1.showWidget("depth", cv::viz::WCloud(depth_color), pose); //, viewpose.translate(cv::Vec3f(4, 0, 0))

        // raycast
        Mat rdp, norm;
        tsdf.rayCast(rdp, norm, viz1.getViewerPose());

        cv::imshow("raycast", rdp);
        cv::imshow("norm", norm);
        cv::imshow("rgb", rgb);

        // cv::imwrite(cv::format("pic/norm%d.png",i), norm);
        // cv::imwrite(cv::format("pic/raycast%d.png",i), rdp);

        frame = cv::Scalar(255, 255, 255);
        cv::cvtColor(norm, norm, cv::COLOR_BGRA2BGR);
        cv::cvtColor(rdp, rdp, cv::COLOR_BGRA2BGR);
        cv::Mat vdep;
        cv::cvtColor(tsdf.leghtdep, vdep, cv::COLOR_BGRA2BGR);

        cvui::image(frame, 10, 10, rgb);
        cvui::image(frame, 10, 20 + rgb.rows, vdep);

        cvui::image(frame, rgb.cols + 20, 10, norm);
        cvui::image(frame, rgb.cols + 20, 20 + norm.rows, rdp);

        // cvui::image(frame, 10, 50, norm);

        // Render UI components to the frame
        cvui::text(frame, 110, 80, "Hello, world!");
        cvui::text(frame, rgb.cols +rgb.cols+40 , 120, cv::format("rec%d",i));

        // Update cvui stuff and show everything on the screen
        cvui::imshow(WINDOW_NAME, frame);

        if (cv::waitKey(20) == 27)
        {
            break;
        }

        // tsdf.save();
        cv::waitKey(5);
        // viz1.spin();
        viz1.spinOnce(false);
    }
    while (1)
    {
        Mat rdp, norm;
        tsdf.rayCast(rdp, norm, viz1.getViewerPose());
        cv::imshow("raycast", rdp);
        cv::imshow("norm", norm);
        viz1.spinOnce(false);
        char key = cv::waitKey(15);
        if (key == 'n')
            break;
    }
    Mat cpu_cloud2;
    Mat cpu_color;
    tsdf.exportCloud(cpu_cloud2, cpu_color);
    SaveVoxelGrid2SurfacePointCloud(cpu_cloud2, cpu_color, "tsdf.ply");
}

int mainh5(int c, char *avg[])
{
    // loguru::g_stderr_verbosity = 9; // print everything
    cudaDeviceReset();
    ck(cudaGetLastError());

    cv::viz::Viz3d viz1("viz1");
    viz1.showWidget("Coordinate", cv::viz::WCoordinateSystem());
    ViewCtl vctl;

    // fx:478.340210
    // fy:478.340210
    // cx:329.211426
    // cy:234.110977

    viz1.registerKeyboardCallback(ViewCtl::KeyboardCallback, &vctl);
    TSDF tsdf(make_uint3(512, 512, 512), make_int2(480, 640));
    tsdf.pintr = new Camera(make_float4(478.340210, 478.340210, 329.211426, 234.110977));
    tsdf.pintr->sca = 0.0001f;

    // cv::Ptr<cv::hdf::HDF5> h5io = cv::hdf::open("imgs.h5");
    cv::Ptr<cv::hdf::HDF5> h5io = cv::hdf::open("/home/u20/图片/CCC/Examples/build/imgs.h5");

    for (int i = 0; i < 100; i++)
    {
        // 读取图片
        // dsread();
        cv::Mat rgb;
        h5io->dsread(rgb, cv::format("/rgb%d", i));

        cv::Mat depth;
        h5io->dsread(depth, cv::format("/depth%d", i));
        cv::imshow("depth", depth);

        cv::imshow("rgb", rgb);
        cv::waitKey(10);
        cv::Affine3f pose = cv::Affine3f::Identity(); // dt.pose.getvectorPose(i);
        tsdf.addScan(depth, rgb, pose);
        // //显示当前TSDF
        // Mat cpu_cloud2;
        // Mat cpu_color;
        // tsdf.exportCloud(cpu_cloud2, cpu_color);
        // viz1.showWidget("depthmode222", cv::viz::WCloud(cpu_cloud2, cpu_color));
        // if (vctl.interactive_mode_)
        //     viz1.setViewerPose(pose);

        // 显示当前帧深度图对应的点云
        Mat depth_color, cpu_cloud;
        tsdf.depth2cam(depth, rgb, depth_color, cpu_cloud, cv::Affine3f::Identity());
        cv::Affine3f viewpose = cv::Affine3f::Identity();
        viz1.showWidget("depth", cv::viz::WCloud(depth_color), pose); //, viewpose.translate(cv::Vec3f(4, 0, 0))

        // raycast
        Mat rdp, norm;
        tsdf.rayCast(rdp, norm, viz1.getViewerPose());

        // cv::imshow("raycast", rdp);
        // cv::imshow("norm", norm);
        // cv::imshow("rgb", rgb);

        // cv::imwrite(cv::format("pic/norm%d.png",i), norm);
        // cv::imwrite(cv::format("pic/raycast%d.png",i), rdp);

        // tsdf.save();
        // viz1.spin();
    }
    // Mat cpu_cloud2;
    // Mat cpu_color;
    // tsdf.exportCloud(cpu_cloud2, cpu_color);
    // SaveVoxelGrid2SurfacePointCloud(cpu_cloud2, cpu_color, "tsdf.ply");
}